﻿using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Linq;
using System.Runtime.InteropServices;

using RayDen.Core;
using RayDen.Core.Primitives;
using RayDen.Entity.Scene;
using RayDen.RayEngine.Core.Types;
using RayDen.RayEngine.Data;
using Wintellect.PowerCollections;

namespace RayDen.RayEngine.IntersectionEngines {
    using Triangle = TriangleInfo;
    using BaseRenderable = TriangleInfo;
    using RayInfo = RayData;
    using RenderableCollection = List<TriangleInfo>;

    public class GpuBvhAccelleration {
        private const int PrimitivesInNode = 1;
        private const int NoPrimitive = int.MaxValue;
        private int costSamples = 4, isectCost = 80, traversalCost = 10;
        int treeType = 2;
        private float emptyBonus = 0.5f;
        private IMaterialProvider materialProvider;

        public Node RootNode;

        #region Internal Types

        public class Node {
            public AABB Bound;
            public Node Left, Right;
            public Axis SplitAxis;
            public int StartOffset, nPrims;
        }

        internal class BVHBuildNode {
            public AABB bounds;
            public BVHBuildNode[] children;
            public int splitAxis, firstOffset, nPrims;

            internal bool IsLeaf {
                get { return children[0] != null && children[1] != null; }
            }
            internal BVHBuildNode() {
                children = new BVHBuildNode[2];
            }
            internal void InitLeaf(int first, int nP, ref AABB box) {
                bounds = box;
                nPrims = nP;
                firstOffset = first;
            }
            internal void InitInterior(BVHBuildNode left, BVHBuildNode right, int axis) {
                children[0] = left;
                children[1] = right;
                splitAxis = axis;
                bounds = left.bounds.Union(right.bounds);
                nPrims = 0;
            }
        }


        internal class LinearBVHNode {
            public AABB bbox;//+12+12+4
            public int primOffset;//+4
            public int secondChildOffset;//+4
            public byte nPrimitives;//+1
            public byte axis;//+1
            public void SetChildOffset(int sc) {
                this.secondChildOffset = sc;
            }
            public void SetOffset(int offset) {
                this.primOffset = offset;
            }
        }

        internal struct BBox {
            public Vector Min;
            public Vector Max;

            public BBox(AABB b) {
                this.Max = b.Max;
                this.Min = b.Min;
            }

            public static bool IntersectBox(ref BBox box, ref RayInfo ray) {
                float t0 = ray.Min;
                float t1 = ray.Max;

                var rayInvDir = ray.InvDirection;
                //1f /ray.Dir;

                float invDirX = rayInvDir.x;
                if (!process_box_coord(ref t0, ref t1, box.Min.x - ray.Org.x, box.Max.x - ray.Org.x, invDirX))
                    return false;

                float invDirY = rayInvDir.y;
                if (!process_box_coord(ref t0, ref t1, box.Min.y - ray.Org.y, box.Max.y - ray.Org.y, invDirY))
                    return false;

                float invDirZ = rayInvDir.z;
                if (!process_box_coord(ref t0, ref t1, box.Min.z - ray.Org.z, box.Max.z - ray.Org.z, invDirZ))
                    return false;
                return true;
            }
            private static bool process_box_coord(ref float t0, ref float t1, float mnRO, float mxRO, float invRayDir) {
                float tNear = (mnRO) * invRayDir;
                float tFar = (mxRO) * invRayDir;
                // Update parametric interval from slab intersection $t$s
                if (tNear > tFar)
                    swap(ref tNear, ref tFar);
                t0 = tNear > t0 ? tNear : t0;
                t1 = tFar < t1 ? tFar : t1;
                if (t0 > t1)
                    return false;
                return true;
            }
            private static void swap(ref float tNear, ref  float tFar) {
                float tmp = tNear;
                tNear = tFar;
                tFar = tmp;
            }
        }

        [StructLayout(LayoutKind.Explicit, Size = 32)]
        internal struct LBVHNode {
            [FieldOffset(0)]
            public BBox Bound;
            [FieldOffset(24)]
            public Int32 PrimOffset;
            [FieldOffset(24)]
            public Int32 SecondChildOffset;
            [FieldOffset(28)]
            public byte PrimitiveCount;
            [FieldOffset(29)]
            public byte Axis;
        }

        internal struct GBvhNode {
            public BBox Bound;
            public Int32 PrimOffset;
            public Int32 SkipOffset;
        }

        #endregion

        private Vector[] sceneVertices;
        private TriangleInfo[] triangles;
        private List<TriangleMesh> meshes;
        private int bvhNodesCount;

        public void BuildTree(List<TriangleInfo> primitives, int start, int end) {
            var nodes = new List<Node>();
            for (int index = 0; index < primitives.Count; index++) {
                var newNode = new Node() {
                    Bound = primitives[index].WorldBound(ref sceneVertices),
                    nPrims = 1,
                    StartOffset = index,
                    Left = null,
                    Right = null
                };
                newNode.Bound.Expand(MathLab.RAY_EPSILON);
                nodes.Add(newNode);
            }
            this.BuildTreeInternal(nodes);
        }

        private void BuildTreeInternal(List<Node> nodes) {
            Tracer.TraceLine("Building accelleration {0}  {1} Node size in bytes", nodes.Count, Marshal.SizeOf(typeof(LBVHNode)));
            RootNode = this.BuildHierarchy(nodes, 0, nodes.Count, 2, 0 );
            Tracer.TraceLine("Hierarchy builded");
            gpuBvhTree = new GBvhNode[nNodes];
            var gpuNodes = this.BuildArray(RootNode, 0);
        }


        static bool bvh_ltf_x(Node n, float v) {
            return n.Bound.Max.x + n.Bound.Min.x < v;
        }

        static bool bvh_ltf_y(Node n, float v) {
            return n.Bound.Max.y + n.Bound.Min.y < v;
        }

        static bool bvh_ltf_z(Node n, float v) {
            return n.Bound.Max.z + n.Bound.Min.z < v;
        }


        private Func<Node, float, bool>[] SplitF = new Func<Node, float, bool>[] { bvh_ltf_x, bvh_ltf_y, bvh_ltf_z };
        private long nNodes;

        Node BuildHierarchy(List<Node> list, int b, int e, int axis, long recDepth) {

            int begin = Math.Min(b, e);
            int end = Math.Max(b, e);
            if (recDepth == 64) {
                Debugger.Break();
            }
            int splitAxis = axis;
            float splitValue;

            nNodes += 1;
            if (end - begin <= 1) // Only a single item in list so return it
                return list[begin];

            Node parent = new Node();
            parent.StartOffset = NoPrimitive;
            parent.Left = null;
            parent.Right = null;

            List<int> splits = new List<int>(treeType + 1);
            splits.Add(begin);
            splits.Add(end);
            for (var i = 2; i <= treeType; i *= 2) { // Calculate splits, according to tree type and do partition
                for (int j = 0, offset = 0; j + offset < i && splits.Count > j + 1; j += 2) {
                    if (splits[j + 1] - splits[j] < 2) {
                        j--;
                        offset++;
                        continue; // Less than two elements: no need to split
                    }

                    FindBestSplit(list, splits[j], splits[j + 1], out splitValue, out splitAxis);

                    //list[begin].StartOffset = begin
                    var ps = begin + splits[j];
                    var pe = (begin + splits[j + 1]);
                    int axis1 = splitAxis;
                    float value = splitValue;
                    var it = Algorithms.StablePartition(list.GetRange(ps,pe - ps), node => SplitF[axis1](node, value));
                    var middle = it - begin;
                    middle = Math.Max(splits[j] + 1, Math.Min(splits[j + 1] - 1, middle)); // Make sure coincidental BBs are still split
                    splits.Insert(splits[0] + j + 1, middle);
                }
            }

            Node child, lastChild;
            // Left Child
            child = BuildHierarchy(list, splits[0], splits[1], splitAxis, recDepth+1);
            parent.Left = child;
            parent.Bound = child.Bound;
            lastChild = child;

            // Add remaining children
            for (var i = 1; i < splits.Count - 1; i++) {
                child = BuildHierarchy(list, splits[i], splits[i + 1], splitAxis, recDepth + 1);
                lastChild.Right = child;
                parent.Bound = AABB.Union(parent.Bound, child.Bound);
                lastChild = child;
            }

            return parent;
        }


        int BuildArray(Node node, int offset) {
            // Build array by recursively traversing the tree depth-first
            while (node != null) {
                var p = gpuBvhTree[offset];

                p.Bound = new BBox(node.Bound);
                p.PrimOffset = node.StartOffset;
                offset = BuildArray(node.Left, offset + 1);
                p.SkipOffset = offset;

                node = node.Right;
            }

            return offset;
        }

        private BVHBuildNode rootNode;

        public void BuildSAH() {

        }

        internal interface INodeComparer {
            bool Compare(TriangleInfo p);
        }

        internal class MidComparer : INodeComparer {

            public int dim;
            public float pMid;
            public Vector[] vertices;

            public bool Compare(TriangleInfo p) {
                return p.Sum(ref vertices)[dim] < pMid;
            }

        }

        private Node BuildSubTree(RenderableCollection primitives, int start, int end) {
            if (start - end == 0)
                return null;
            AABB bound = primitives.GetRange(start, end - start).Select(prim => prim.WorldBound(ref sceneVertices)).Aggregate((b1, b2) => b1.Union(b2));
            var node = new Node() { Bound = bound, SplitAxis = (Axis)bound.MaximumExtent() };
            bvhNodesCount++;
            if ((end - start) <= PrimitivesInNode) {
                node.StartOffset = start;
                node.nPrims = end - start;
                //node.Primitives = primitives.GetRange(start, (end - start));
                return node;
            }

            float pmid, split;
            int axis = bound.MaximumExtent();

            //FindBestSplit(primitives, start, end, out split, out axis);

            //var pmid = .5f * (bound.Min[axis] + bound.Max[axis]);
            FindBestSplit(primitives.GetRange(start, end - start), start, end, out pmid, out axis);
            var cmp = new MidComparer() { dim = axis, pMid = pmid, vertices = sceneVertices };
            var mid =
                Algorithms.Partition(primitives.GetRange(start, end - start), cmp.Compare);
            //(start + end) / 2;
            node.Left = this.BuildSubTree(primitives, start, mid);
            node.Right = this.BuildSubTree(primitives, mid, end);
            return node;
        }

        private List<LinearBVHNode> nodes;
        private LBVHNode[] bvhTree;
        private GBvhNode[] gpuBvhTree;

        private int FlattenBVHTree(Node node, ref int offset) {
            if (node == null) {
                return -1;
            }
            nodes[offset] = new LinearBVHNode() { bbox = node.Bound };
            var linearNode = nodes[offset];
            int myOffset = (offset)++;
            if (node.nPrims > 0) {
                linearNode.primOffset = node.StartOffset;
                linearNode.nPrimitives = (byte)node.nPrims;
            }
            else {
                linearNode.axis = (byte)node.SplitAxis;
                linearNode.nPrimitives = 0;
                FlattenBVHTree(node.Left, ref offset);
                linearNode.secondChildOffset = FlattenBVHTree(node.Right, ref offset);
            }
            return myOffset;
        }

        private void FlattenTree() {
            int offset = 0;
            nodes = Enumerable.Range(0, bvhNodesCount).Select(i => new LinearBVHNode()).ToList();
            this.FlattenBVHTree(RootNode, ref offset);
            bvhTree = nodes.Select(linearBvhNode => linearBvhNode.nPrimitives > 0
                                                        ? new LBVHNode() {
                                                            Axis = linearBvhNode.axis,
                                                            Bound = new BBox(linearBvhNode.bbox),
                                                            PrimOffset = linearBvhNode.primOffset,
                                                            PrimitiveCount = linearBvhNode.nPrimitives
                                                        }
                                                        : new LBVHNode() {
                                                            Axis = linearBvhNode.axis,
                                                            Bound = new BBox(linearBvhNode.bbox),
                                                            PrimitiveCount = linearBvhNode.nPrimitives,
                                                            SecondChildOffset = linearBvhNode.secondChildOffset
                                                        }).ToArray();
            RootNode = null;
            nodes.Clear();
            CreateGpuBvhTree();
        }

        private void CreateGpuBvhTree() {
            this.gpuBvhTree = bvhTree.Select(lbvhNode =>
                lbvhNode.PrimitiveCount > 0 ? new GBvhNode() { Bound = lbvhNode.Bound, PrimOffset = lbvhNode.PrimOffset, SkipOffset = lbvhNode.SecondChildOffset }
                                            : new GBvhNode() { Bound = lbvhNode.Bound, SkipOffset = lbvhNode.SecondChildOffset, PrimOffset = lbvhNode.PrimOffset }
                ).ToArray();
        }

        #region BVH Implementation

        public int TraversalStackSize = 32;
        public bool IntersectLBVH(ref RayInfo ray, ref RayHit sp) {
            int currentNode = 0;
            float u = 0f, v = 0f, t1 = float.MaxValue, dist = float.MaxValue;
            bool hit = false;
            int stopNode = gpuBvhTree[0].SkipOffset;
            sp.Index = RayBuffer.NotHit;

            while (currentNode < stopNode) {
                if (BBox.IntersectBox(ref gpuBvhTree[currentNode].Bound, ref ray)) {
                    if (gpuBvhTree[currentNode].PrimOffset != 0) {
                        if (triangles[gpuBvhTree[currentNode].PrimOffset].Intersect(sceneVertices, ref ray, ref t1, ref u,
                                                                                  ref v)) {
                            var triangleIndex = gpuBvhTree[currentNode].PrimOffset;
                            dist = t1;
                            sp.Index = (uint)(triangleIndex);
                            sp.Distance = dist;
                            sp.U = u;
                            sp.V = v;
                            hit = true;
                        }
                    }
                    currentNode++;
                }
                else {
                    currentNode = gpuBvhTree[currentNode].SkipOffset;
                }
            }
            return hit;
        }

        private bool IntersectBVH(ref RayInfo ray, ref RayHit sp) {
            if (bvhTree.Length == 0)
                return false;

            int triangleIndex = -1;
            float u = 0f, v = 0f;
            bool hit = false;
            Vector invDir = ray.InvDirection;
            //new Vector(1f / ray.Dir.x, 1f / ray.Dir.y, 1f / ray.Dir.z);
            var dirIsNeg = new[] { invDir.x < 0, invDir.y < 0, invDir.z < 0 };
            int todoOffset = 0, nodeNum = 0;
            int[] todo = new int[TraversalStackSize];
            var dist = float.MaxValue;
            while (true) {
                var node = bvhTree[nodeNum];
                float t1 = 0f;
                if (BBox.IntersectBox(ref node.Bound, ref ray)) {
                    //if (node.bbox.Intersect(vray, out t0, out t2)) {
                    if (node.PrimitiveCount > 0) {
                        for (var i = 0; i < node.PrimitiveCount; ++i) {
                            if (triangles[node.PrimOffset + i].Intersect(sceneVertices, ref ray, ref t1, ref u, ref v)) {
                                if (dist > t1) {
                                    triangleIndex = node.PrimOffset + i;
                                    dist = t1;
                                    sp.Index = (uint)(triangleIndex);
                                    sp.Distance = dist;
                                    sp.U = u;
                                    sp.V = v;
                                    hit = true;
                                }
                            }
                        }
                        if (todoOffset == 0)
                            break;
                        nodeNum = todo[--todoOffset];
                    }
                    else {
                        if (dirIsNeg[node.Axis]) {
                            todo[todoOffset++] = nodeNum + 1;
                            nodeNum = node.SecondChildOffset;
                        }
                        else {
                            todo[todoOffset++] = node.SecondChildOffset;
                            nodeNum = nodeNum + 1;
                        }
                    }
                }
                else {
                    if (todoOffset == 0)
                        break;
                    nodeNum = todo[--todoOffset];
                }
            }

            if (hit) {
                return true;
            }
            sp.Index = RayBuffer.NotHit;

            return false;
        }

        private bool ShadowIntersectBVH(ref RayInfo ray) {
            if (nodes.Count == 0)
                return false;
            bool hit = false;
            Vector invDir = ray.InvDirection;
            //new Vector(1f / ray.Dir.x, 1f / ray.Dir.y, 1f / ray.Dir.z);
            var dirIsNeg = new[] { invDir.x < 0, invDir.y < 0, invDir.z < 0 };
            int todoOffset = 0, nodeNum = 0;
            int[] todo = new int[TraversalStackSize];
            while (true) {
                var node = bvhTree[nodeNum];
                float t1 = 0f, u = 0f, v = 0f;
                if (BBox.IntersectBox(ref node.Bound, ref ray)) {
                    if (node.PrimitiveCount > 0) {
                        for (var i = 0; i < node.PrimitiveCount; ++i) {
                            if (!triangles[node.PrimOffset + i].Intersect(sceneVertices, ref ray, ref t1, ref u, ref v))
                                continue;
                            hit = true;
                            break;
                        }
                        if (todoOffset == 0)
                            break;
                        nodeNum = todo[--todoOffset];
                    }
                    else {
                        if (dirIsNeg[node.Axis]) {
                            todo[todoOffset++] = nodeNum + 1;
                            nodeNum = node.SecondChildOffset;
                        }
                        else {
                            todo[todoOffset++] = node.SecondChildOffset;
                            nodeNum = nodeNum + 1;
                        }
                    }
                }
                else {
                    if (todoOffset == 0)
                        break;
                    nodeNum = todo[--todoOffset];
                }
            }

            return hit;
        }
        #endregion

        #region IAccellerationStructure Members

        public void Init(RayEngineScene scene, IMaterialProvider materialProvider) {
            var sceneModel = scene;
            meshes = new List<TriangleMesh>();
            meshes.AddRange(scene.Meshes);
            this.sceneVertices = sceneModel.Vertices;
            AABB sceneBound = sceneVertices.Select(i => new AABB(i)).Aggregate((b1, b2) => b1.Union(b2));
            var ltriangles = scene.Triangles;
            Comparison<Triangle> tc = (t1, t2) => CompareTriangles(t1, t2,
                (Axis)sceneBound.MaximumExtent()
                //                Axis.PLANE_Z
                );
            ltriangles.Sort(tc);


            this.materialProvider = materialProvider;
            Tracer.TraceLine("Building tree ");
            this.BuildTree(ltriangles, 0, ltriangles.Count);
            Tracer.TraceLine("BVH builded {0} nodes {1} object in leaf", bvhNodesCount, PrimitivesInNode);
            Tracer.TraceLine("Flattening tree...");

            this.triangles = ltriangles.ToArray();
            FlattenTree();
            Tracer.TraceLine("Complete");
        }

        public int CompareTriangles(Triangle t1, Triangle t2, Axis a = Axis.PLANE_Z) {
            var p1 = t1.Center(ref sceneVertices);
            var p2 = t2.Center(ref sceneVertices);
            return p1.Component(a).CompareTo(p2.Component(a));
        }

        public bool Intersect(ref RayInfo ray) {
            return ShadowIntersectBVH(ref ray);
            //return IntersectNode(RootNode, ray);

        }



        public bool Intersect(ref RayInfo ray, out RayHit intersection) {
            intersection = new RayHit();
            return IntersectLBVH(ref ray, ref intersection);
            //return IntersectBVH(ref ray, ref intersection);
            //return IntersectNode(RootNode, ray, out intersection);
        }

        private bool IntersectNode(Node node, ref RayInfo ray) {
            if (node == null) {
                return false;
            }
            float t0, t1 = 0f, u = 0f, v = 0f;
            if (IntersectBox(node.Bound, ref ray)) {
                //ray.MinT = t0;
                // ray.MaxT = t1;
                if (node.nPrims > 0) {
                    for (var np = node.StartOffset; np < (node.StartOffset + node.nPrims); np++) {
                        if (triangles[np].Intersect(sceneVertices, ref ray, ref t1, ref u, ref v)) {
                            return true;
                        }
                    }
                }
                return IntersectNode(node.Left, ref ray) || IntersectNode(node.Right, ref ray);
            }
            return false;
        }

        public static bool IntersectBox(AABB box, ref RayData ray) {
            float t0 = ray.Min;
            float t1 = ray.Max;
            float hitt0 = t0;
            float hitt1 = t1;

            var rayInvDir = ray.InvDirection;
            //1f /ray.Dir;

            float invDirX = rayInvDir.x;
            if (!process_box_coord(ref t0, ref t1, box.Min.x - ray.Org.x, box.Max.x - ray.Org.x, invDirX))
                return false;

            float invDirY = rayInvDir.y;
            if (!process_box_coord(ref t0, ref t1, box.Min.y - ray.Org.y, box.Max.y - ray.Org.y, invDirY))
                return false;

            float invDirZ = rayInvDir.z;
            if (!process_box_coord(ref t0, ref t1, box.Min.z - ray.Org.z, box.Max.z - ray.Org.z, invDirZ))
                return false;

            /*
            for (int i = 0; i < 3; ++i) {
                // Update interval for _i_th bounding box slab
                float invRayDir = 1f / ray.Direction[i];
                if (!process_box_coord(ref t0, ref t1, min[i] - ray.Origin[i], max[i] - ray.Origin[i], invRayDir)) return false;
            }
             * */
            //ray.MinT = t0;
            //ray.MaxT = t1;
            hitt0 = t0;
            hitt1 = t1;
            return true;
        }
        private static bool process_box_coord(ref float t0, ref float t1, float mnRO, float mxRO, float invRayDir) {
            float tNear = (mnRO) * invRayDir;
            float tFar = (mxRO) * invRayDir;
            // Update parametric interval from slab intersection $t$s
            if (tNear > tFar) swap(ref tNear, ref tFar);
            t0 = tNear > t0 ? tNear : t0;
            t1 = tFar < t1 ? tFar : t1;
            if (t0 > t1) return false;
            return true;
        }

        private static void swap(ref float tNear, ref  float tFar) {
            float tmp = tNear;
            tNear = tFar;
            tFar = tmp;
        }
        private void FindBestSplit(List<Triangle> list, int begin, int end, out float splitValue, out int bestAxis) {
            splitValue = 0;
            if (end - begin == 2) {
                splitValue = (list[begin].WorldBound(ref sceneVertices).Max[0] + list[begin].WorldBound(ref sceneVertices).Min[0] + list[end - 1].WorldBound(ref sceneVertices).Max[0] + list[end - 1].WorldBound(ref sceneVertices).Min[0]) / 2f;
                bestAxis = 0;
            }
            else {
                Vector mean2 = new Vector(), var = new Vector();

                for (int i = begin; i < end; i++)
                    mean2 += list[i].WorldBound(ref sceneVertices).Max + list[i].WorldBound(ref sceneVertices).Min;
                mean2 /= end - begin;

                for (int i = begin; i < end; i++) {
                    Vector v = list[i].WorldBound(ref sceneVertices).Max + list[i].WorldBound(ref sceneVertices).Min - mean2;
                    v.x *= v.x;
                    v.y *= v.y;
                    v.z *= v.z;
                    var += v;
                }
                // Select axis with more variance
                if (var.x > var.y && var.x > var.z)
                    bestAxis = 0;
                else if (var.y > var.z)
                    bestAxis = 1;
                else
                    bestAxis = 2;

                if (costSamples > 1) {
                    AABB nodeBounds = new AABB();//!!!!
                    for (int i = begin; i < end; i++)
                        nodeBounds = nodeBounds.Union(list[i].WorldBound(ref sceneVertices));

                    Vector d = nodeBounds.Max - nodeBounds.Min;
                    float totalSA = (2f * (d.x * d.y + d.y * d.z + d.z * d.x));
                    float invTotalSA = 1f / totalSA;

                    // Sample cost for split at some points
                    float increment = 2f * d[bestAxis] / (costSamples + 1);
                    float bestCost = float.PositiveInfinity;
                    for (float splitVal = 2f * nodeBounds.Min[bestAxis] + increment; splitVal < 2f * nodeBounds.Max[bestAxis]; splitVal += increment) {
                        int nBelow = 0, nAbove = 0;
                        AABB bbBelow = new AABB(Vector.Zero), bbAbove = new AABB(Vector.Zero);
                        for (int j = begin; j < end; j++) {
                            if ((list[j].WorldBound(ref sceneVertices).Max[bestAxis] + list[j].WorldBound(ref sceneVertices).Min[bestAxis]) < splitVal) {
                                nBelow++;
                                bbBelow = bbBelow.Union(list[j].WorldBound(ref sceneVertices));
                            }
                            else {
                                nAbove++;
                                bbAbove = bbAbove.Union(list[j].WorldBound(ref sceneVertices));
                            }
                        }
                        Vector dBelow = bbBelow.Max - bbBelow.Min;
                        Vector dAbove = bbAbove.Max - bbAbove.Min;
                        float belowSA = 2f * ((dBelow.x * dBelow.y + dBelow.x * dBelow.z + dBelow.y * dBelow.z));
                        float aboveSA = 2f * ((dAbove.x * dAbove.y + dAbove.x * dAbove.z + dAbove.y * dAbove.z));
                        float pBelow = belowSA * invTotalSA;
                        float pAbove = aboveSA * invTotalSA;
                        float eb = (nAbove == 0 || nBelow == 0) ? emptyBonus : 0;
                        float cost = traversalCost + isectCost * (1f - eb) * (pBelow * nBelow + pAbove * nAbove);
                        // Update best split if this is lowest cost so far
                        if (cost < bestCost) {
                            bestCost = cost;
                            splitValue = splitVal;
                        }
                    }
                }
                else {
                    // Split in half around the mean center
                    splitValue = mean2[bestAxis];
                }
            }
        }

        private void FindBestSplit(List<Node> list, int begin, int end, out float splitValue, out int bestAxis) {
            splitValue = 0;
            if (end - begin == 2) {
                splitValue = (list[begin].Bound.Max[0] + list[begin].Bound.Min[0] + list[end - 1].Bound.Max[0] + list[end - 1].Bound.Min[0]) / 2f;
                bestAxis = 0;
            }
            else {
                Vector mean2 = new Vector(), var = new Vector();

                for (int i = begin; i < end; i++)
                    mean2 += list[i].Bound.Max + list[i].Bound.Min;
                mean2 /= end - begin;

                for (int i = begin; i < end; i++) {
                    Vector v = list[i].Bound.Max + list[i].Bound.Min - mean2;
                    v.x *= v.x;
                    v.y *= v.y;
                    v.z *= v.z;
                    var += v;
                }
                // Select axis with more variance
                if (var.x > var.y && var.x > var.z)
                    bestAxis = 0;
                else if (var.y > var.z)
                    bestAxis = 1;
                else
                    bestAxis = 2;

                if (costSamples > 1) {
                    AABB nodeBounds = new AABB();//!!!!
                    for (int i = begin; i < end; i++)
                        nodeBounds = nodeBounds.Union(list[i].Bound);

                    Vector d = nodeBounds.Max - nodeBounds.Min;
                    float totalSA = (2f * (d.x * d.y + d.y * d.z + d.z * d.x));
                    float invTotalSA = 1f / totalSA;

                    // Sample cost for split at some points
                    float increment = 2f * d[bestAxis] / (costSamples + 1);
                    float bestCost = float.PositiveInfinity;
                    for (float splitVal = 2f * nodeBounds.Min[bestAxis] + increment; splitVal < 2f * nodeBounds.Max[bestAxis]; splitVal += increment) {
                        int nBelow = 0, nAbove = 0;
                        AABB bbBelow = new AABB(Vector.Zero), bbAbove = new AABB(Vector.Zero);
                        for (int j = begin; j < end; j++) {
                            if ((list[j].Bound.Max[bestAxis] + list[j].Bound.Min[bestAxis]) < splitVal) {
                                nBelow++;
                                bbBelow = bbBelow.Union(list[j].Bound);
                            }
                            else {
                                nAbove++;
                                bbAbove = bbAbove.Union(list[j].Bound);
                            }
                        }
                        Vector dBelow = bbBelow.Max - bbBelow.Min;
                        Vector dAbove = bbAbove.Max - bbAbove.Min;
                        float belowSA = 2f * ((dBelow.x * dBelow.y + dBelow.x * dBelow.z + dBelow.y * dBelow.z));
                        float aboveSA = 2f * ((dAbove.x * dAbove.y + dAbove.x * dAbove.z + dAbove.y * dAbove.z));
                        float pBelow = belowSA * invTotalSA;
                        float pAbove = aboveSA * invTotalSA;
                        float eb = (nAbove == 0 || nBelow == 0) ? emptyBonus : 0;
                        float cost = traversalCost + isectCost * (1f - eb) * (pBelow * nBelow + pAbove * nAbove);
                        // Update best split if this is lowest cost so far
                        if (cost < bestCost) {
                            bestCost = cost;
                            splitValue = splitVal;
                        }
                    }
                }
                else {
                    // Split in half around the mean center
                    splitValue = mean2[bestAxis];
                }
            }
        }
        #endregion

        internal LBVHNode[] GetData() {
            return this.bvhTree;
        }
    }
}
